#include "armor.h"
#include "../../../tool/include/util_func.h"

#include <cmath>

using namespace cv;
using namespace std;

Armor::Armor() = default;

Armor::~Armor() = default;

// void Armor::setNumberImg() {
//     cv::Rect2d rect_left = lbar_rect.boundingRect();
//     cv::Rect2d rect_right = rbar_rect.boundingRect();
//     int min_x, min_y, max_x, max_y;
//     // 左右的x坐标取二八分界点作为灯条和数字的分割线
//     min_x = int(fmin(rect_left.x + 0.5 * rect_left.width, rect_right.x + 0.5 * rect_right.width));
//     max_x = int(fmax(rect_left.x + 0.5 * rect_left.width, rect_right.x + 0.5 * rect_right.width));
//     min_y = int(fmin(rect_left.y, rect_right.y) - 0.5 * (rect_left.height + rect_right.height) / 2.0);
//     max_y = int(fmax(rect_left.y + rect_left.height, rect_right.y + rect_right.height) +
//                 0.5 * (rect_left.height + rect_right.height) / 2.0);
//     int temp_width = max_x - min_x;
//     int temp_height = max_y - min_y;
//     preventExceed(min_x, min_y, temp_width, temp_height, src);
//     if (temp_height > 0 && temp_width > 0) {
//         number_img = src(cv::Rect(min_x, min_y, temp_width, temp_height));
//         cv::cvtColor(number_img, number_img, cv::COLOR_BGR2GRAY);
//         resize(number_img, number_img, cv::Size(Classify::img_width, Classify::img_height), INTER_AREA);
//     } else {
//         number_img = cv::Mat::zeros(Classify::img_height, Classify::img_width, CV_8UC1);
//     }
// }

int Armor::getNumber() { return classifier_num; }

void Armor::setPriority()
{
    // 根据距离瞄准中心的距离设置打击优先级
    priority = std::abs(armor_rect.center.x / img_width - 0.5);
    priority += [=](auto y) -> double
    {
        if (y < 0.15)
            return 0;
        else
            return y * 0.5;
    }(std::abs(armor_rect.center.y / img_height - 0.5));
}

bool Armor::scoreComparator(const Armor &a, const Armor &b) { return a.sum_error_score < b.sum_error_score; }

bool Armor::priorityComparator(const Armor &a, const Armor &b) { return a.priority < b.priority; }

bool Armor::sort_diff_position_func(const Armor &a, const Armor &b) { return a.diff_position < b.diff_position; }

cv::Rect Armor::rect() { return armor_rect.boundingRect(); }

void Armor::preventExceed(int &x, int &y, int &width, int &height, const cv::Mat &src)
{
    if (x < 0)
        x = 0;
    if (y < 0)
        y = 0;
    if (x + width > src.cols)
        width = src.cols - x;
    if (y + height > src.rows)
        height = src.rows - y;
}

void Armor::assignMemberVariables(const Lightbar &lbar, const Lightbar &rbar, const Pose &pose)
{
    float delta_width = fabs(rbar.rect.center.x - lbar.rect.center.x);
    float delta_height = fabs(rbar.rect.center.y - lbar.rect.center.y);
    double armor_width = sqrt(delta_width * delta_width + delta_height * delta_height);
    double armor_height = (lbar.rect.size.height + rbar.rect.size.height) / 2.0;
    double armor_ratio = armor_width / armor_height;
    double armor_bar_len_ratio = lbar.rect.size.height / rbar.rect.size.height;
    armor_bar_len_ratio = ((armor_bar_len_ratio > 1.0) ? armor_bar_len_ratio : (1.0 / armor_bar_len_ratio));

    this->lbar_rect = lbar.rect;
    this->rbar_rect = rbar.rect;
    this->height = (lbar.rect.size.height + rbar.rect.size.height) / 2.0;
    std::vector<cv::Point> armor_vertices;
    Util::extractPoints(this->lbar_rect, armor_vertices);
    Util::extractPoints(this->rbar_rect, armor_vertices);
    this->armor_rect = cv::minAreaRect(armor_vertices);
    adjustArmorRect(armor_rect);
    this->ratio = armor_ratio;
    this->bar_len_ratio = armor_bar_len_ratio;
    this->bar_delta_angle = abs(lbar.angle - rbar.angle);
    this->isBigArmor = armor_ratio > 3.4;
    this->armor_angle = fabs(atan2(delta_height, delta_width) * 180 / Util::PI);

    target.z = g_camera_internal_matrix.at<double>(1, 1) * 0.055 / armor_height;
    target.x = target.z * (armor_rect.center.x - 0.5 * img_width) / g_camera_internal_matrix.at<double>(0, 0);
    target.y = target.z * (armor_rect.center.y - 0.5 * img_height) / g_camera_internal_matrix.at<double>(1, 1);
    target.x = target.x + X_OFFSET / 1000.0;
    target.y = target.y + Y_OFFSET / 1000.0;
    target.z = target.z + Z_OFFSET / 1000.0;
    Util::wheel2land(target, pose); /* 转换到大地坐标系 */
    target.yaw = Util::getYawForPTZ(target);
    target.clock = pose.mcu_time;
}

void Armor::assignMemberVariables(const array<Point2f, 4> &coordinates, const Pose &pose)
{
    Point2f left_center = {(coordinates[0].x + coordinates[1].x) / 2, (coordinates[0].y + coordinates[1].y) / 2};
    Point2f right_center = {(coordinates[2].x + coordinates[3].x) / 2, (coordinates[2].y + coordinates[3].y) / 2};
    Point2f top_center = {(coordinates[0].x + coordinates[3].x) / 2, (coordinates[0].y + coordinates[3].y) / 2};
    Point2f bottom_center = {(coordinates[2].x + coordinates[1].x) / 2, (coordinates[2].y + coordinates[1].y) / 2};
    double armor_width = std::abs(right_center.x - left_center.x);
    double armor_height = std::abs(bottom_center.y - top_center.y);
    double armor_ratio = armor_width / armor_height;
    this->isBigArmor = armor_ratio > 3.4;
    double armor_bar_len_ratio = (coordinates[1].y - coordinates[0].y) / (coordinates[2].y - coordinates[3].y);
    armor_bar_len_ratio = ((armor_bar_len_ratio > 1.0) ? armor_bar_len_ratio : (1.0 / armor_bar_len_ratio));
    this->armor_rect = cv::minAreaRect(coordinates);
    adjustArmorRect(armor_rect);
    this->ratio = armor_ratio;
    this->bar_len_ratio = armor_bar_len_ratio;
    this->bar_delta_angle = std::abs(atan2(coordinates[1].y - coordinates[0].y, coordinates[1].x - coordinates[0].x) -
                                     atan2(coordinates[2].y - coordinates[3].y, coordinates[2].x - coordinates[3].x));
    this->armor_angle = std::abs(
        atan2(left_center.y - right_center.y, left_center.x - right_center.x) * 180 / Util::PI);

    target.z = g_camera_internal_matrix.at<double>(1, 1) * 0.055 / armor_height;
    target.x = target.z * (armor_rect.center.x - 0.5 * img_width) / g_camera_internal_matrix.at<double>(0, 0);
    target.y = target.z * (armor_rect.center.y - 0.5 * img_height) / g_camera_internal_matrix.at<double>(1, 1);
    target.x = target.x + X_OFFSET / 1000.0;
    target.y = target.y + Y_OFFSET / 1000.0;
    target.z = target.z + Z_OFFSET / 1000.0;
    Util::wheel2land(target, pose); /* 转换到大地坐标系 */
    target.clock = pose.mcu_time;
    // 若识别结果是英雄，就是大装甲板，若识别结果是基地/前哨，就按照装甲板比例判断是否大装甲板（因为分类网络会误分类二者）
    // this->isBigArmor = classifier_num == 1 || classifier_num == 6 || ((classifier_num == 8 || classifier_num == 7) && armor_ratio > 3.4);
    // this->isBigArmor = classifier_num == 1 || ((classifier_num == 8 || classifier_num == 7) && armor_ratio > 3.4);//2023哨兵
    this->isBigArmor = classifier_num == 1 || classifier_num == 8 || classifier_num == 7; // 联盟赛写死基地为大装甲板
    this->coord_points = std::move(coordinates);
}

void adjustArmorRect(cv::RotatedRect &rect)
{
    if (rect.size.width < rect.size.height)
        rect = cv::RotatedRect(rect.center, cv::Size2f(rect.size.height, rect.size.width), rect.angle + 90);
}

void Armor::listInfo()
{
    std::cout << "amor_rect_area: " << this->armor_rect.size.area() << std::endl;
    std::cout << "armor_ratio: " << this->ratio << std::endl;
    std::cout << "bar_delta_angle; " << this->bar_delta_angle << std::endl;
    std::cout << "armor_angle: " << this->armor_angle << std::endl;
}
